

#include "at_cmd_callback.h"
#include "HeaderFiles.h"


void userSetSpeedM1(char *param);
void userSetSpeedM2(char *param);
void getBuildTime(char *param);
void sysTestCtrl(char *parma);
void EDL7141read(char *param);
void wheelspeedset(char *param);
void printfwall(char *param);
void printfcliff(char *param);
void printfbumper(char *param);
void printfdrop(char *param);
void printfflow(char *param);
void printfIMU(char *param);
void printfinfrared(char *param);
void printfkey(char *param);
void printfvoltage(char *param);
void printfcurrent(char *param);
void printfspeed(char *param);
void motorctrlset(char *param);
void printfencoder(char *param);
void setCtrlmode(char *param);
void faultclear(char *param);
void flowmodeset(char *param);
void printfbattery(char *param);

ATList_st stATCmdList[AT_CMD_NUMBER] =
{
     {"AT+SPEEDM1=" , userSetSpeedM1},
     {"AT+SPEEDM2=" , userSetSpeedM2},
     {"AT+BUILDTIME",  getBuildTime},
	 {"AT+SYSCTRL=",   sysTestCtrl},
	 {"AT+6EDREGREAD=", EDL7141read},
	 {"AT+WHEELCTRL=",  wheelspeedset},
	 {"AT+PRINTFWALL=",   printfwall},
	 {"AT+PRINTFCLIFF=",  printfcliff},
	 {"AT+PRINTFBUMPER=", printfbumper},
	 {"AT+PRINTFDROP=",  printfdrop},
	 {"AT+PRINTFFLOW=",  printfflow},
	 {"AT+PRINTFIMU=",  printfIMU},
	 {"AT+PRINTFINFR=", printfinfrared},
	 {"AT+PRINTFKEY=",  printfkey},
	 {"AT+PRINTFVOL=",  printfvoltage},
	 {"AT+PRINTFCUR=",  printfcurrent},
	 {"AT+PRINTFSPEED=", printfspeed},
	 {"AT+MOTORCTRL=",   motorctrlset},
	 {"AT+PRINTFENCODER=",printfencoder},
	 {"AT+SETCTRLMODE=", setCtrlmode},
	 {"AT+FAULTCLEAR=", faultclear},
	 {"AT+FLOWMODESET=", flowmodeset},
	 {"AT+PRINTFBAT=", printfbattery}

};


void userSetSpeedM1(char *param)
{
    int16_t speed;

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        UART_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }

    speed = atoi(result[0]);

    if((speed > 3000) || (speed < -3000))
    {
        UART_PrintfMesg("erro: motor1 speedset too high \r\n");
        return;
    }
    else if(speed != 0)
    {
			
		#if 0
        userHdl[HAL_MTR_1].ctrl.speedRef_rpm = speed;
        userHdl[HAL_MTR_1].ctrl.enable = 1;
		#endif

        UART_PrintfMesg("motor1 runing speedset = %d rpm \r\n",speed);

    }
    else
    {
		#if 0
        userHdl[HAL_MTR_1].ctrl.speedRef_rpm = speed;
        userHdl[HAL_MTR_1].ctrl.enable = 0;
		#endif

        UART_PrintfMesg("motor1 stop ctrl \r\n");
    }

}


void userSetSpeedM2(char *param)
{

    UART_PrintfMesg("SPEEDM2 \r\n");

}



void getBuildTime(char *param)
{
	
	int16_t software_vesion = userHdl.param.software_version_num;
	int16_t hardware_vesion = userHdl.param.hardware_version_num;
	
	debug_PrintfMesg("Software version: %d.%d.%d     ",(software_vesion/100),(software_vesion%100/10),(software_vesion%100%10));
	debug_PrintfMesg("Hardware version: %d.%d.%d \r\n",(hardware_vesion/100),(hardware_vesion%100/10),(hardware_vesion%100%10));
	
    debug_PrintfMesg("Project Build Time: %s %s \r\n",__DATE__,__TIME__);
}



void sysTestCtrl(char *param)
{
	uint16_t cmd_enable = 0;

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        UART_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	cmd_enable = atoi(result[0]);
	
	
	if(cmd_enable == 0)
	{
		userHdl.ctrl.enable = 0;
		
		UART_PrintfMesg("system stop ctrl \r\n");
		
	}
	
	else if(cmd_enable == 1)
	{
		userHdl.ctrl.enable = 1;
		
		UART_PrintfMesg("system runing... \r\n");
	}
	
	else if(cmd_enable == 2)
	{
	
	
	}
}


void EDL7141read(char *param)
{

	uint16_t addr = 0;
	uint16_t reslt = 0;
	uint16_t motor_num  = 0;
	
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(2 != param_num)
    {
        UART_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }

	motor_num = atoi(result[0]);
    addr = atoi(result[1]);
	
	reslt = 7;

	UART_PrintfMesg("motor %d 6EDL7141 reg %d rslt 0x%x  \r\n", motor_num, addr, reslt);

}

void wheelspeedset(char *param)
{

//	float32_t anglespeed_ref = 0;
//	float32_t linespeed_ref = 0;
	
//    char *result[AT_MAX_PARAM] = {NULL};
//    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
//    if(2 != param_num)
//    {
//        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
//        return;
//    }
	
//	anglespeed_ref = atoi(result[0]);
//	linespeed_ref  = atoi(result[1]);
	
	
//	userHdl.ctrl.speedangle_rads = anglespeed_ref * 0.01f;
//	userHdl.ctrl.speedline_mps = linespeed_ref *0.01f;
	
}

void printfwall(char *param)
{
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_WALL] = atoi(result[0]);
}


void printfcliff(char *param)
{
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_CLIFF] = atoi(result[0]);
}



void printfbumper(char *param)
{
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_BUMPER] = atoi(result[0]);
}

void printfdrop(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_DROP] = atoi(result[0]);
}



void printfflow(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_LIGHTFLOW] = atoi(result[0]);
}

void printfIMU(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_IMU] = atoi(result[0]);
}

void printfinfrared(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_INFRARED] = atoi(result[0]);
}


void printfkey(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SIGNAL_KEY] = atoi(result[0]);
}
	

void printfvoltage(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SIGNAL_VOLTAGE] = atoi(result[0]);
}
	
void printfcurrent(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SIGNAL_CURRENT] = atoi(result[0]);
}
	
void printfspeed(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SIGNAL_SPEED] = atoi(result[0]);
}

void motorctrlset(char *param)
{
	char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(5 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
//	userHdl.ctrl.enable_edgebrush = atoi(result[0]);
//	userHdl.ctrl.enable_mainbrush = atoi(result[1]);
//	userHdl.ctrl.enable_dustfan   = atoi(result[2]);
//	userHdl.ctrl.enable_leftmop   = atoi(result[3]);
//	userHdl.ctrl.enable_rightmop  = atoi(result[4]);
//	
//	if(userHdl.ctrl.enable_edgebrush)
//	{
//		debug_PrintfMesg("left and right edge brush enable \r\n");
//	}
//	
//	if(userHdl.ctrl.enable_mainbrush)
//	{
//		debug_PrintfMesg("roll edge brush enable \r\n");
//	}
//	
//	if(userHdl.ctrl.enable_dustfan)
//	{
//		debug_PrintfMesg("dust fan enable \r\n");
//	}
//	
//	if(userHdl.ctrl.enable_leftmop)
//	{
//		debug_PrintfMesg("left mop enable \r\n");
//	}
//	
//	if(userHdl.ctrl.enable_rightmop)
//	{
//		debug_PrintfMesg("right mop enable \r\n");
//	}
	
	
}	


void printfencoder(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SENSOR_ENCODER] = atoi(result[0]);
}

void printfbattery(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	userHdl.assis.printfenable[SIGNAL_BATTERY] = atoi(result[0]);
}


void setCtrlmode(char *param)
{
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
//	userHdl.ctrl.ctrlmode = atoi(result[0]);
	
	//if(userHdl.ctrl.ctrlmode == CTRL_MODE_DEBUG)
	{
		//userHdl.signal.ledstatus = LED_DEBUG;	
	}
}


void faultclear(char *param)
{
	uint16_t faultclear_en = 0;
	
    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
	faultclear_en = atoi(result[0]);
	
	if(faultclear_en == 1)
	{
		memset((void *)&userHdl.fault.all,0,sizeof(userHdl.fault.all));
		memset((void *)&userHdl.warn.all ,0,sizeof(userHdl.warn.all));
//		memset((void *)&userHdl.diag     ,0,sizeof(userHdl.diag));
//		
//		userHdl.ctrl.systemstop = 0;
	}
}

void flowmodeset(char *param)
{

    char *result[AT_MAX_PARAM] = {NULL};
    uint16_t param_num = search_param_in_str(param,result,AT_MAX_PARAM);
    if(1 != param_num)
    {
        debug_PrintfMesg("error,%d;",ERR_AT_PUB_PARAM_NUM);
        return;
    }
	
//	userHdl.ctrl.lightmode = atoi(result[0]);
	
	//set_lightflow_mode(userHdl.ctrl.lightmode);

}
